#ifndef KALMAN_H__
#define KALMAN_H__

#include "types.h"
#include "coords.h"
#include <Eigen/Dense>

namespace common
{
	/// Класс "фильтр Калмана"
	class KalmanFilter
	{
	public:
		explicit KalmanFilter();
		virtual ~KalmanFilter();
		
		/// Инициировать состояние
		void setInitialState(
			const RectCoords& a_coords, ///< Координаты
			const RectCoords& a_vel,    ///< Скорость 
			const Covariance& a_P,      ///< Начальная ковариация состояния
			Time a_time                 ///< Начальное время
			)
		{
			m_x << a_coords.getAsArray(), a_vel.getAsArray();
			m_P = a_P;
			m_time = a_time;
		}

		/// Задать ковариацию неопределенности состояния
		void setStateUncertainity(const Covariance& a_Q) { m_Q = a_Q; }

		/// Инициировать ковариацию неопределенности измерения
		void setMeasureUncertainity(const Covariance& a_R) { m_R = a_R; }

		/// Обновить состояние фильтра
		void updateState(
			const RectCoords& a_measure, ///< координаты измерения \f$z_k\f$
			Time a_time ///< время измерения \f$T_m\f$
			);
		
		/// Извлечь текущее время
		Time getT() const { return m_time; }

		/// Извлечь текущие величины координат 
		RectCoords getCoords() const;

		/// Извлечь текущие величины скорости
		RectCoords getVel() const;

		// Извлечь ковариацию неопределенности состояния
		const Covariance& getP() const { return m_P; }

	private:
		KalmanFilter& operator=(const KalmanFilter&);
		KalmanFilter(const KalmanFilter&);

		/// Текущее время \f$T\f$ [c]
		Time m_time;

		/// Текущий вектор состояния \f$\hat{x}_{k|k}\f$
		StateVector m_x;
		
		/// Ошибка предсказания вектора состояния \f$P_{k|k}\f$
		Covariance m_P;

		/// Ковариация шума измерения \f$R\f$
		Covariance m_R;

		/// Ковариация шума состояния \f$Q\f$
		Covariance m_Q;
	};
};

#endif // KALMAN_H__